#include "plugins/action/find_heel_strike.hpp"
#include "utils/timer_utils.hpp"

BT::NodeStatus FindHeelStrike::onStart()  {
    std::cout << "FindHeelStrike_onStart(): " << this->name() << std::endl;
    //从端口获取T_fid_
    if (!getInput<uint64_t>("T_fid", T_fid_)) {
        throw BT::RuntimeError("Missing required input 'FindHeelStrike.T_fid'");
    }
    //从端口的指针获取mean_T_hs_potential_region数组的数据
    if (!getInput<std::shared_ptr<std::vector<uint64_t>>>("mean_T_hs_potential_region", mean_T_hs_potential_region_ptr)) {
        throw BT::RuntimeError("Missing required input 'FindHeelStrike.mean_T_hs_potential_region'");
    }
        if (mean_T_hs_potential_region_ptr) {
            mean_T_hs_potential_region_ = *mean_T_hs_potential_region_ptr;
        } else {
            std::cout << "mean_T_hs_potential_region_ptr is null!" << std::endl;
        }
    //从端口的指针获取mean_swing_duration_数组的数据
    if (!getInput<std::shared_ptr<std::vector<uint64_t>>>("mean_swing_duration", mean_swing_duration_ptr)) {
        throw BT::RuntimeError("Missing required input 'FindHeelStrike.mean_swing_duration_'");
    }
        if (mean_swing_duration_ptr) {
            mean_swing_duration_ = *mean_swing_duration_ptr;
        } else {
            std::cout << "mean_swing_duration_ptr is null!" << std::endl;
        }
    //从端口获取T_to_
    if (!getInput<uint64_t>("T_to", T_to_)){
        throw BT::RuntimeError("Missing required input 'FindHeelStrike.T_to'");
    }       
    print_all_inports(true);

    cnt = 0;
    this->onRunning();
    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus FindHeelStrike::onRunning() {
    /*从端口获取数据*/
    if (!getInput<sensorData>("sensor_input", tmp_sensor_data_filtered_)) {
        throw BT::RuntimeError("Missing required input 'FindHeelStrike.sensor_input'");
    }
    /*获取当前时间ms*/
    // current_millis_ = std::chrono::duration_cast<std::chrono::milliseconds>(
    //     std::chrono::steady_clock::now().time_since_epoch()).count();

    current_millis_ = config().blackboard->get<uint64_t>("global_counter");
    current_millis_ *= 2;

    F_gy_ = tmp_sensor_data_filtered_.imu_gyr;
    buffer_gyr_.push_back(F_gy_);//保存数据，用于potential_region_起始
    buffer_time_label_.push_back(current_millis_);
    // /*测试代码片段*/
    // std::cout<<"ApproachObject_onRunning"<<std::endl;
    // cnt++;
    // if (cnt > 3)
    // {
    //     return BT::NodeStatus::SUCCESS;
    // }
    // return BT::NodeStatus::RUNNING;

    // 获取数组T_hs_potential_region最后一个数值
    last_T_hs_potential_region = mean_T_hs_potential_region_.back();//52
    // config().blackboard->set("HeelStrike_last_T_hs_potential_region", last_T_hs_potential_region); //写入到黑板

    /*功能实现*/
    if (current_millis_-T_fid_ > 0.7 * last_T_hs_potential_region){//current_millis_ -  6544 >  0.7 * 52 //current_millis_ > 6580.4
        // std::cout << "FindHeelStrike last_T_hs_potential_region: " << 0.7 *last_T_hs_potential_region << std::endl;
        if (F_gy_last_ > 0 && F_gy_ <= 0)//potential_region_结束
        {
            //当前时刻点
            T_hs_realtime_ = current_millis_;
            std::cout << COLOR_YELLOW << "FindHeelStrike_T_hs_realtime_: " << T_hs_realtime_ << COLOR_RESET << std::endl;

            if (buffer_gyr_.empty() || buffer_time_label_.empty()) {
                throw std::runtime_error("FindHeelStrike Error: Cannot get max value from an empty array!");}
            auto R_gy_iter = std::max_element(buffer_gyr_.begin(), buffer_gyr_.end());//寻找mean_T_hs_potential_region_的最大值作为T_hs_
            R_gy = *R_gy_iter;
            R_gy_index_ = std::distance(buffer_gyr_.begin(), R_gy_iter); // 计算迭代器偏移量
            T_hs_ = buffer_time_label_[R_gy_index_]; //mean_T_hs_potential_region_的最大值时间点作为T_hs_
            std::cout << COLOR_YELLOW << "FindHeelStrike_T_hs_: " << T_hs_ << COLOR_RESET << std::endl;
            // FindHeelStrike_T_hs_T_to_:182964028 1371
            swing_duration_ = T_hs_ - T_to_; //下一步更新到均值的数组中,应该是600，当前是2708
            // new_value_update_mean(swing_duration_, mean_swing_duration_); //将新值追加到mean_T_hs_potential_region_数组
            mean_swing_duration_.push_back(swing_duration_); //将新值追加到mean_swing_duration_数组
            // config().blackboard->set("FindHeelStrike_swing_duration_", swing_duration_);

            setOutput("T_hs", T_hs_); // 开始脚跟着地,todo 给出heelstrike的标志位
            setOutput("mean_swing_duration", std::make_shared<std::vector<uint64_t>>(mean_swing_duration_)); //将数组通过端口发布出去

            if (T_hs_ - T_fid_ < 0 || T_hs_ - T_fid_ > swing_duration_){ 
                // todo 其他异常处理，  所有的运算都判断一下是否合理，此处要求 mean_T_hs_potential_region_ 小于摆动相
                // 构造详细的错误信息
                auto difference = T_hs_ - T_fid_;
                std::string errorMsg = "T_hs_ - T_fid_ is not positive! "
                                    "T_hs_ = " + std::to_string(T_hs_) + 
                                    ", T_fid_ = " + std::to_string(T_fid_) + 
                                    ", difference = " + std::to_string(difference);
                // 输出到控制台（明显报错）
                std::cerr << "❌ CRITICAL ERROR: " << errorMsg << std::endl;
                std::cerr << "🚫 Program will exit due to invalid calculation." << std::endl;
                // 保存到日志文件
                log_error_to_file(errorMsg);
                log_error_to_file("Program terminated due to invalid T_hs_potential_region calculation");
                return BT::NodeStatus::FAILURE;
            }

            T_hs_potential_region_ = T_hs_ - T_fid_; //是不是这么计算的，下一步更新到均值上
            mean_T_hs_potential_region_.push_back(T_hs_potential_region_); //将新值追加到mean_T_hs_potential_region_数组
            //to do 求一下均值
            setOutput("mean_T_hs_potential_region", std::make_shared<std::vector<uint64_t>>(mean_T_hs_potential_region_)); //将数组指针通过端口发布出去
            // std::cout<< COLOR_RED << "FindHeelStrike,_T_hs_potential_region_: " << mean_T_hs_potential_region_.size() <<  COLOR_RESET<< std::endl;
            this-> print_all_outports(true);
            //一定要清空，否则查找最大值，会找到上一个步态周期的缓存数据
            buffer_gyr_.clear();
            buffer_time_label_.clear();
            return BT::NodeStatus::SUCCESS;
        }

        F_gy_last_ = F_gy_;
        return BT::NodeStatus::RUNNING;
    }
    else if (current_millis_ - T_fid_ > last_T_hs_potential_region){
        //tof信息落地后，但是没有在规定时间发现脚跟抬起， todo 如何处理
        std::cout<<COLOR_BLUE<<"T_hs_potential_region_timeout"<<COLOR_RESET<<std::endl;
    }
    else{
        F_gy_last_ = F_gy_;
        return BT::NodeStatus::RUNNING;
    }
    F_gy_last_ = F_gy_;
    return BT::NodeStatus::RUNNING;
}



void FindHeelStrike::new_value_update_mean(uint64_t new_value, std::vector<uint64_t>& mean_history, size_t window_size){
    if (mean_history.empty()) {
        mean_history.push_back(new_value);
        return;
    }
    uint64_t current_mean = mean_history.back();
    size_t current_size = std::min(mean_history.size(), window_size);
    
    uint64_t new_mean = (current_mean * current_size + new_value) / (current_size + 1);
    mean_history.push_back(new_mean);
    
    // 定期清理，避免频繁的 erase 操作
    if (mean_history.size() > window_size * 1.2) {
        // 只保留最近 window_size 个元素
        if (mean_history.size() > window_size) {
            mean_history.erase(mean_history.begin(), mean_history.begin() + (mean_history.size() - window_size));
        }
    }
}

void FindHeelStrike::print_vector(const std::vector<uint64_t>& vec, const std::string& name) {
    if (!vec.empty()) {
        std::cout << name << ": [";
        for (size_t i = 0; i < vec.size(); ++i) {
            if (i > 0) std::cout << ", ";
            std::cout << vec[i];
        }
        std::cout << "]" << std::endl;
    } else {
        std::cout << COLOR_RED <<name << ": 向量为空" << COLOR_RESET << std::endl;
    }
}
void FindHeelStrike::print_var(const uint64_t& var, const std::string& name) {
    std::cout << name << ": " << var << std::endl;
}



void FindHeelStrike::print_all_outports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== FindHeelStrike节点端口输出值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_hs_, "T_hs_");
    print_vector(mean_T_hs_potential_region_, "mean_T_hs_potential_region_");
    print_vector(mean_swing_duration_, "mean_swing_duration_");
    std::cout << "======================" << std::endl;
}
void FindHeelStrike::print_all_inports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== FindHeelStrike节点端口输入值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_fid_, "T_fid_");
    print_var(T_to_, "T_to_");
    print_vector(mean_T_hs_potential_region_, "mean_T_hs_potential_region_");
    print_vector(mean_swing_duration_, "mean_swing_duration_");
    // 打印所有向量
    // print_vector(a, "a");
    std::cout << "======================" << std::endl;
}

















#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  std::cout << "Registering nodes...: FindHeelStrike" << std::endl;//在宏内添加调试信息或条件判断
  factory.registerNodeType<FindHeelStrike>("FindHeelStrike");
}